function cbn_k = PostureUpdate(cbn_k_1,v_k_1n,position_k_1,data_k_1,data_k)
%已知量：上一时刻的姿态、上一时刻的速度、上一时刻的纬度和高程
%已知量：前一次和这一次的角度变化量
%利用方向余弦矩阵姿态更新
%%
%变量初始化
    phi_k_1 =position_k_1(1);
    h_k_1   =position_k_1(3);
    dTheta_k_1 =[data_k_1(2); data_k_1(3); data_k_1(4)];
    dTheta_k   =[data_k(2); data_k(3); data_k(4)];

%%
%姿态更新
    Phik  = EquRVbGet(dTheta_k,dTheta_k_1);
    Zetak = EquRVnGet(phi_k_1,h_k_1,v_k_1n);
    cbb   = EquRVtoC(Phik,'b');
    cnn   = EquRVtoC(Zetak,'n');

    cbn_k = cnn*cbn_k_1*cbb;
end

%辅助函数
function Phi=EquRVbGet(dTheta_k,dTheta_k_1)
%已知量：前一次和这一次的角度变化量
%双子样假设计算b系姿态变化对应的等效旋转矢量
    Phi=dTheta_k+(1/12.0)*cross(dTheta_k_1,dTheta_k);

end

function Zeta=EquRVnGet(phi_k_1,h_k_1,v_k_1n)
%已知量：上一时刻的速度、上一时刻的纬度和高程
%计算n系姿态变化对应的等效旋转矢量
    detat = 0.005;
    wien  = wienGet(phi_k_1);
    R     = rmnGet(phi_k_1);
    wenn  = wennGet(v_k_1n,R,h_k_1,phi_k_1);
    Zeta  = (wien+wenn)*detat;

end
%%
function A = skewMatrix(v)
%得到向量对应的反对称矩阵
%输入v是一个3x1的列向量
    A = [   0  , -v(3),  v(2);
           v(3),   0  , -v(1);
          -v(2),  v(1) ,  0 ];
end

function c=EquRVtoC(EquPV,type)
%已知量：等效旋转矢量
%由等效旋转矢量得到姿态变换矩阵
    module=norm(EquPV);
    skewM=skewMatrix(EquPV);
    I=eye(3);%姿态变换矩阵是3*3
if(type=='b')
    m=1;
elseif(type=='n')
    m=-1;
end
    c=I+m*(sin(module) * skewM)/module + ((1-cos(module)) * skewM * skewM)/module^2;

end
%%
function R = rmnGet(phi)
%获得某个纬度处的子午圈半径：RM、卯酉圈半径：RN;
    a=6378137.0;
    e=0.08181919104;
    RM=(a * (1 - e^2)) / (1 - e^2 * sin(phi)^2)^(3/2);
    RN=a / sqrt(1 - e^2 * sin(phi)^2);
    R=[RM; RN];
end

function wien = wienGet(phi)
%获得地球自转角速度在n系下的投影
    we=7.292115E-5;
    wien=[we*cos(phi); 0; -we*sin(phi)];
end

function wenn = wennGet(v,R,h,phi)
%获得载体相对地球角速度在n系下的投影
    RM=R(1);
    RN=R(2);
    vN=v(1);
    vE=v(2);
    wenn=[vE / (RN + h); -vN /(RM + h); -vE * tan(phi)/(RN + h);];
end